/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Robot.*;
import Robot.Sensors.*;
import Utils.*;

/**
 *
 * @author alejo
 */
public class navegacionUS {
    
    public static void main(String args[])
    {

        boolean isSimulated = true;
        boolean showArbFrame = true;
        boolean showMapObstFrame = true;


        Robot real;
        //Creacion de la instancia robot.
        if(isSimulated)
            real = new Robot("real","localhost",6665, 0);
        else
            real = new Robot("real","192.168.2.146",6665, 0);
        
        RealPosition2d posReal;
        
        
        try
        {
            posReal = new RealPosition2d(real,"realPos",0);
        }
        catch(Exception e)
        {
            e.printStackTrace();
            return;
        }

        Sonar ir;

        try{
            ir = new Sonar(real,"realIR",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }

          

        real.runThreaded(-1, -1);

        while(!((Sonar)ir).updatePoses());
        
        
        posReal.setMotorPower(true);

        Pose2d initialPose;

        if(isSimulated)
            initialPose = new Pose2d((float)-6.5f,(float)-6.5f,(float)Math.PI/4);
        else
            initialPose = new Pose2d(0.0f,0.0f,0);

        posReal.setPosition(initialPose);

        if(isSimulated)
        {
            posReal.setMaxSpeed(0.4f);
            posReal.setRotControl(0.3f,1.0f);
        }
        else
        {
            posReal.setMaxSpeed(1.0f);
            posReal.setRotControl(0.2f,0.5f);
        }



        //punto de meta
        Vector2d point;

        if(isSimulated)
            point=new Vector2d(5,5,Vector2d.MODE_XY);
        else
            point=new Vector2d(2.0f,0.0f,Vector2d.MODE_XY);


        //comportamientos
        MSGoToPoint goto2 = new MSGoToPoint("goto",posReal,point,MSGoToPoint.MODE_EXPONENTIAL);


        PSDetectObstacles obstReal;

        obstReal = new PSDetectObstaclesSonar("obstReal",posReal, ir, false  );

        obstReal.setMaxRange(2);
        obstReal.setMinRange(0.2f);

        PSMapObstacles obstMapped = new PSMapObstacles("mapObstacles",obstReal, posReal,2.0f,-1,showMapObstFrame);


        MSAvoidObstacles avoidReal = new MSAvoidObstacles("avoidReal",obstMapped, posReal,MSAvoidObstacles.MODE_EXPONENTIAL);


        avoidReal.setExponentialDistance(0.3f);

        MSAvoidObstaclesVFH avoid = new MSAvoidObstaclesVFH("avoidVFH",obstMapped,goto2,posReal,MSAvoidObstacles.MODE_LINEAR,true);

        avoid.setValleyTresh(1.3f, 0.7f);
        avoid.setExponentialDistance(0.3f);


        //gains
        goto2.setGain(0.6f);
        avoidReal.setGain(0.4f);
        avoid.setGain(1.0f);
        
        Schema[] array = {avoid,avoidReal};

        VectorAdditionArbitrator arbitrator1=new VectorAdditionArbitrator(array,posReal,showArbFrame);
        
        arbitrator1.enabled = true;
        
        while(true)
        {
         
            arbitrator1.step();
    
            try
            {
                Thread.sleep(50);
            }
            catch(Exception e)
            {}
        }
        
    }

}
